clc 
clear all


data1 = readtable('/home/maoli/Desktop/Dynamic_ParaEstimation_v3/cali_legs_v3_real_3.csv');
data = table2array(data1);
data_q = data(:,6:10)';
data_qd = data(:,16:20)';
data_q_des = data(:,36:40)';
data_qd_des = data(:,46:50)';
data_tau = data(:,26:30)';

data2 = readtable('/home/maoli/Desktop/Dynamic_ParaEstimation_v3/cali_legs_v3_real_comp_fric_1.csv');
data_ = table2array(data2);
data_q_ = data_(:,6:10)';
data_qd_ = data_(:,16:20)';
data_q_des_ = data_(:,36:40)';
data_qd_des_ = data_(:,46:50)';
data_tau_ = data_(:,26:30)';



figure(1)
plot(data_q','r')
hold on
plot(data_q_','b-.')
hold on
plot(data_q_des','g')


figure(2)
plot(data_qd','r')
hold on
plot(data_qd_','b-.')
hold on
plot(data_qd_des','g')